; fe3ts.asm
; FE3 Task Switcher

	.include "fe3def.inc"
	.include "fe3util.inc"
	.include "vicrom.inc"

	.export init

; I/O base addresses
VIC_BASE = $9000
VIA1_BASE = $9110
VIA2_BASE = $9120

; Some VIA registers
REG_ORB = 0
REG_ORA = 1
REG_DDRB = 2
REG_DDRA = 3
REG_IFR = 13
REG_IER = 14

; Basic configuration
MAX_ENV = 8
FIRST_BANK = 1

; Structure holding CPU status
.struct PStatus
	ac		.byte
	xr		.byte
	yr		.byte
	flags	.byte
	sp		.byte
	ier1	.byte
	ier2	.byte
.endstruct

; File magic
FILE_MAGIC = $AA01
	
	.segment "ZEROPAGE"
	
ptr:		.res 2

	.segment "ENTRY"
	
init:		jmp do_init
switcher:	jmp do_switch_env0
init_env:	jmp do_init_env
switch_env:	jmp do_switch_env
write_env:	jmp do_write_env
read_env:	jmp do_read_env

retadr:		.res 2			; align on even address!

	.segment "DATA"
	
used:		.res MAX_ENV

high_bank:	.res MAX_ENV

low_bank:	.res MAX_ENV
low_page:	.res MAX_ENV

cur_env:	.res 1
next_env:	.res 1

status:		.tag PStatus

status_loc:	.res 2
stack_loc:	.res 2

	.segment "RODATA"

; Start addresses of memory copies
pageadr:	.byte $20, $40, $60, $A0

; Autostart code
a0cbm:		.word treset
			.word a0nmi
			.byte $41, $30, $C3, $C2, $CD

	a0cbm_len = * - a0cbm 

	.segment "CODE"
	
;
;	Initialise Task Switcher
;	
	
do_init:	ldx #0
			; init used flag
@init_loop:	lda #0
			sta used, x
			; init high bank
			txa
			clc
			adc #FIRST_BANK
			sta high_bank, x
			; init low mem storage pointer
			txa
			lsr
			lsr
			clc
			adc #FIRST_BANK+MAX_ENV
			sta low_bank, x
			txa
			and #3
			tay
			lda pageadr, y
			sta low_page, x
			; next
			inx
			cpx #MAX_ENV
			bcc @init_loop
			; end of loop
			ldx #0
			stx cur_env
			jsr save_env
			ldx #0
			lda high_bank, x
			ora #SUPER_RAM_MODE
			sta FE3_REG1
			inx
			stx used
			rts

;
;	Initialise environment
;	INPUT:
;		A ... Environment index
;
			
do_init_env:
			cmp #MAX_ENV		; index out of range?
			bcs @ill_op
			tax
			beq @ill_op			; don't reset root env

			php					; save interrupt flag
			sei					; disable interrupts
			lda low_bank, x		; select storage
			ora #SUPER_RAM_MODE
			sta FE3_REG1
			
			lda low_page, x		; clear memory area
			sta ptr+1
			lda #0
			sta ptr
			stx next_env
			ldx #$20			; no. of pages in area					
			tay
@clrlp:		sta (ptr), y
			iny
			bne @clrlp
			inc ptr+1
			dex
			bne @clrlp
			ldx next_env
			
			lda low_page, x		; set tos to reset vec.
			clc		
			adc #$11
			sta ptr+1
			ldy #$FE
			lda #<(treset-1)
			sta (ptr), y
			iny
			lda #>(treset-1)
			sta (ptr), y
			
			lda status_loc		; set-up status ptr
			sta ptr
			lda status_loc+1
			clc
			adc low_page, x
			sta ptr+1
			
			ldy #PStatus::sp
			lda #$FD
			sta (ptr), y
			ldy #PStatus::flags
			php
			pla
			sta (ptr), y

			lda high_bank, x
			ora #SUPER_RAM_MODE
			sta FE3_REG1
			
			ldy #a0cbm_len-1	; Set-up autostart @ $A000
@cpa0:		lda a0cbm, y
			sta $A000, y
			dey
			bpl @cpa0
			
			lda #1
			sta used, x
			
			ldx cur_env			; return to current env.
			lda high_bank, x
			ora #SUPER_RAM_MODE
			sta FE3_REG1
			
			plp					; reset interrupt flag & clc
;			clc
			rts

@ill_op:	sec
			rts
			
do_switch_env0:
			lda #0
;
;	Switch to environment
;	INPUT:
;		A ... Environment index
;
do_switch_env:
			cmp #MAX_ENV		; index out of range?
			bcs @ill_op
			sta next_env
			php
			sei
			sta status+PStatus::ac	; save regs
			stx status+PStatus::xr
			sty status+PStatus::yr
			ldx #$7F				; save & clear IERs
			lda VIA1_BASE+REG_IER
			stx VIA1_BASE+REG_IER
			sta status+PStatus::ier1
			lda VIA2_BASE+REG_IER
			stx VIA2_BASE+REG_IER
			sta status+PStatus::ier2
			pla
			sta status+PStatus::flags
			tsx
			stx status+PStatus::sp
			jsr save_env
			lda next_env
			sta cur_env
			jsr load_env			; don't use stack til txs!
			ldx cur_env
			lda high_bank, x
			ora #SUPER_RAM_MODE
			sta FE3_REG1
			lda status+PStatus::flags
			ldx status+PStatus::sp
			txs
			pha
@rtnonmi:
			lda status+PStatus::ier2
			sta VIA2_BASE+REG_IER
			lda status+PStatus::ier1
			sta VIA1_BASE+REG_IER
			lda status+PStatus::ac
			ldx status+PStatus::xr
			ldy status+PStatus::yr
			plp			; not usable to return from interrupt!
@ill_op:	rts			; rts adds +1 to return address

;
;	Save environment to FE3 RAM
;	INPUT:
;		cur_env ... environment index
;

save_env:	lda #0				; save zeropage
			jsr page2buf
			; select storage
			ldx cur_env
			lda low_bank, x
			ora #SUPER_RAM_MODE
			sta FE3_REG1
			; save $1000-$1FFF
			ldy #0
			lda #$10
			sty $5F				; set src start
			sta $60
			lda #$20
			sty $5A				; set src end
			sta $5B
			clc
			lda low_page, x		
			adc #$10
			pha
			sty $58				; set target end
			sta $59
			jsr BLTUC			; block move
			; copy zeropage
			pla
			pha
			jsr buf2page
			; save $0100-$03FF
			ldy #0
			lda #$01
			sty $5F				; set src start
			sta $60
			lda #$04
			sty $5A				; set src end
			sta $5B
			clc
			pla					
			adc #$04
			pha
			sty $58				; set target end
			sta $59
			jsr BLTUC			; block move
			; save $9400-$97FF (Color RAM)
			ldy #0
			lda #$94
			sty $5F				; set src start
			sta $60
			lda #$98
			sty $5A				; set src end
			sta $5B
			clc
			pla					
			adc #$04
			pha
			sty $58				; set target end
			sta $59
			jsr BLTUC			; block move
			; save I/O regs
			pla
			ldy #0
			sty ptr
			sta ptr+1
@vic_loop:	lda VIC_BASE, y
			sta (ptr), y
			iny
			cpy #16
			bcc @vic_loop
			; TODO: vias

			; Processor status
			ldx cur_env
			lda ptr+1
			sec
			sbc low_page, x
			sty status_loc
			sta status_loc+1
			ldx #0
@cpu_loop:	lda status, x
			sta (ptr), y
			iny
			inx
			cpx #.sizeof(PStatus)
			bcc @cpu_loop
			rts

;
;	Load environment from FE3 RAM
;	INPUT:
;		cur_env ... environment index
;

load_env:	; save tos
			pla
			sta retadr
			pla
			sta retadr+1
			; select storage
			ldx cur_env
			lda low_bank, x
			ora #SUPER_RAM_MODE
			sta FE3_REG1
			; restore $1000-$1FFF
			ldy #0
			lda low_page, x
			sty $5F				; set src start
			sta $60
			clc
			adc #$10
			sty $5A				; set src end
			sta $5B
			pha
			lda #$20
			sty $58				; set target end
			sta $59
			jsr BLTUC			; block move
			; copy zeropage
			pla
			pha
			jsr page2buf
			; restore $0200-$03FF
			ldy #0
			pla
			clc					; skip zeropage & stack
			adc #1
			sty stack_loc
			sta stack_loc+1
			adc #1				
			sty $5F				; set src start
			sta $60
			clc
			adc #2
			pha
			sty $5A				; set src end
			sta $5B
			lda #$04
			sty $58				; set target end
			sta $59
			jsr BLTUC			; block move
			; save $9400-$97FF (Color RAM)
			ldy #0
			pla					
			sty $5F				; set src start
			sta $60
			clc
			adc #$04
			pha
			sty $5A				; set src end
			sta $5B
			lda #$98
			sty $58				; set target end
			sta $59
			jsr BLTUC			; block move
			; save I/O regs
			pla
			ldx #0
			stx ptr
			sta ptr+1
			ldy #0
@vic_loop:	lda (ptr), y
			sta VIC_BASE, y
			iny
			cpy #16
			bcc @vic_loop
			; TODO: vias
			; restore processor status
			ldx #0 
@cpu_loop:	lda (ptr), y
			sta status, x
			iny
			inx
			cpx #.sizeof(PStatus)
			bcc @cpu_loop
			; restore zeropage
			lda #0
			jsr buf2page
			; restore stack
			lda stack_loc
			sta ptr
			lda stack_loc+1
			sta ptr+1
			ldy #0
@stack_lp:	lda (ptr), y
			sta $0100, y
			iny
			bne @stack_lp
			; jump to tos
			inc retadr
			bne @noinchi
			inc retadr+1
@noinchi:	jmp (retadr)

;
;	NMI handler - switch to task 0 if RESTORE pressed.
;

nmi_handler:
			pha
			txa
			pha
			tya
			pha
			lda VIA1_BASE+REG_IFR
			bpl @done
			and VIA1_BASE+REG_IER
			tax
			and #2
			beq @rs232
			jsr do_switch_env0
			bit VIA1_BASE+REG_ORA	; ack NMI
@done:		jmp $FF56
@rs232:		jmp $FEDE

;
;	Reset Task
;

treset:		ldx #$FF
			sei
			txs
			cld
			jsr $FD8D			; Detect RAM
			jsr $FD52			; Set I/O vectors
			lda #<nmi_handler	; Install custom NMI handler
			ldx #>nmi_handler
			sta $0318
			stx $0319
			jsr $FDF9			; Init I/O registers (VIA)
			jsr $E518			; Init VIC & clear screen
			cli
			jsr $E45B			; Set BASIC vectors
			jsr $E3A4			; Init zeropage
			jsr $E404			; Print start message
			
			lda cur_env			; Add env. no. to title screen
			clc
			adc #'0'
			sta $1000+21
			
			jmp $E381			; Init SP & goto READY.

;
;	($A002) - NMI handler
;

a0nmi:		jsr do_switch_env0
			jmp $FF56

	.segment "DATA"

page_cnt:	.res 1
file_no:	.res 1

write_8k:	sta @rd+2
@rd:		lda $F000
			jsr BSOUT
			bcs @done
			inc @rd+1
			bne @rd
			inc @rd+2
			jsr show_bar
			inc page_cnt
			lda page_cnt
			and #$1F
			bne @rd
			clc				
@done:		lda @rd+2
			rts

read_8k:	sta @wr+2
@loop:		jsr BASIN
			bcs @done
@wr:		sta $F000
			inc @wr+1
			bne @loop
			inc @wr+2
			jsr show_bar
			inc page_cnt
			lda page_cnt
			and #$1F
			bne @loop
			clc				
@done:		lda @wr+2
			rts

	.segment "CODE"

;
;	Write environment to disk
;	INPUT:
;		A ... Environment index
;		X ... File number
;	OUTPUT:
;		A ... Pages written
;
do_write_env:
			sta next_env
			stx file_no
			lda #0
			sta page_cnt
			jsr init_bar
			ldx file_no
			jsr CKOUT
			bcs @fin
			
			ldx next_env
			cpx #MAX_ENV		; index out of range?
			bcs @fin
			lda used, x
			beq @fin

			lda #<FILE_MAGIC	; write header bytes
			jsr BSOUT
			lda #>FILE_MAGIC
			jsr BSOUT
			
			ldx next_env
			lda low_bank, x		; write env. low RAM
			ora #SUPER_RAM_MODE
			sta FE3_REG1
			lda low_page, x
			jsr write_8k

			ldx next_env
			lda high_bank, x	; write env. RAM bank
			ora #SUPER_RAM_MODE
			sta FE3_REG1
			
			lda #$20
			jsr write_8k
			jsr write_8k
			jsr write_8k
			lda #$A0
			jsr write_8k
			
			ldx cur_env			; return to current env.
			lda high_bank, x
			ora #SUPER_RAM_MODE
			sta FE3_REG1
			
@fin:		jsr CLRCH
			lda page_cnt
			rts

;
;	Read environment from disk
;	INPUT:
;		A ... Environment index
;		X ... File number
;	OUTPUT:
;		A ... Pages read
;
do_read_env:
			sta next_env
			stx file_no
			lda #0
			sta page_cnt
			jsr init_bar
			ldx file_no
			jsr CHKIN
			bcs @fin

			ldx next_env
			cpx #MAX_ENV		; index out of range?
			bcs @fin
			lda used, x
			beq @fin

			jsr BASIN
			cmp #<FILE_MAGIC	; check header bytes
			bne @fin
			jsr BASIN
			cmp #>FILE_MAGIC
			bne @fin

			ldx next_env
			lda low_bank, x		; read env. low RAM
			ora #SUPER_RAM_MODE
			sta FE3_REG1
			lda low_page, x
			jsr read_8k

			ldx next_env
			lda high_bank, x	; read env. RAM bank
			ora #SUPER_RAM_MODE
			sta FE3_REG1
			
			lda #$20
			jsr read_8k
			jsr read_8k
			jsr read_8k
			lda #$A0
			jsr read_8k
			
			ldx cur_env			; return to current env.
			lda high_bank, x
			ora #SUPER_RAM_MODE
			sta FE3_REG1

@fin:		jsr CLRCH
			lda page_cnt
			rts

;
;	Simple progress
;

	.segment "RODATA"

toscr:		.byte $13, $3E
			.repeat 20
				.byte $20
			.endrep
			.byte $3C, 0

progsym:	.byte $65, $74, $75, $61, $F6, $EA, $E7, $A0

	.segment "ZEROPAGE"

vptr:		.res 2	

	.segment "CODE"

init_bar:	ldx #1			; set-up bar ptr
			lda $D9
			and #$7F
			stx vptr
			sta vptr+1
			lda #<toscr
			ldy #>toscr
			jmp STROUT

show_bar:	lda page_cnt
			tax
			lsr
			lsr
			lsr
			tay
			txa
			and #7
			tax
			lda progsym, x
			sta (vptr), y
			rts
